package estrategia;

import javax.vecmath.Point3d;

import comunicacion.Conector;
import comunicacion.ParametrosEntorno;

public class Utiles {
	
	private Conector entorno;
	
	public Utiles(Conector entorno) {
		this.entorno = entorno; 
	}
	
	/*** Realiza la suma de vectores ***/
	public Vector sumaVectores(Vector vectorA, Vector vectorB) {
		// Pasamos de polar a escalar
		if(vectorA.getMagnitud()==0){ return vectorB;}
		if(vectorB.getMagnitud()==0){ return vectorA;}
		//double xa = (double)(Math.cos(vectorA.getDireccion() * Math.PI/180)*vectorA.getMagnitud());
		//double xb = (double)(Math.cos(vectorB.getDireccion() * Math.PI/180)*vectorB.getMagnitud());
		double xa = (double)(Math.cos(vectorA.getDireccion())*vectorA.getMagnitud());
		double xb = (double)(Math.cos(vectorB.getDireccion())*vectorB.getMagnitud());
		
		//double ya = (double)(Math.sin(vectorA.getDireccion() * Math.PI/180)*vectorA.getMagnitud());
		//double yb = (double)(Math.sin(vectorB.getDireccion() * Math.PI/180)*vectorB.getMagnitud());
		double ya = (double)(Math.sin(vectorA.getDireccion())*vectorA.getMagnitud());
		double yb = (double)(Math.sin(vectorB.getDireccion())*vectorB.getMagnitud());
		
		double xt = xa + xb;
		double yt = ya + yb;
		
		// pasamos de escalar a polar
		double radianes = 0;
		if (xt == 0) {
			radianes = Math.signum(yt)*Math.PI/2;
		} else 
			radianes = (double)(Math.atan(yt/xt));
		double direccion = radianes * (180/Math.PI);
		// Averiguamos el cuadrante
		if((xt>=0) && (yt>=0)){
			// Primer cuadrante
			//direccion = direccion;
		} else if ((xt<0)&&(yt>=0)) {
			// Segundo cuadrante
			direccion = 180 + direccion;			
		} else if ((xt<0)&&(yt<0)){
			// Tercer cuadrante
			direccion = -180 + direccion;
		} else {
			// Cuarto cuadrante
			direccion = direccion;
		}
			
		double magnitud = (double)(Math.sqrt((xt*xt)+(yt*yt)));
		
		Vector vectorSuma = new Vector(magnitud, Math.toRadians(direccion));
		return vectorSuma;
	}

	
	
	
	/**
	 * Calcula la velocidad lineal necesaria segun el metodo de velocidades lineales de Lyapunov
	 * dadas las posiciones de origen (actual) y destino (a la que quiero llegar)
	 */
	double velocidadLinealLV(Point3d origen, Point3d destino) {
		double gamma = ParametrosEntorno.GAMMA;
		
		// Velocidad lineal de Lyapunov
		// En este caso es una modificacion, usamos la raiz de la distancia 
		// para tratar de suavizar la diferencia, achicando las grandes y agrandando las muy chicas
		return gamma * Math.sqrt(distanciaEuclidea(origen, destino));
	}
		
	
	/**
	 * Calcula la velocidad angular necesaria segun el metodo de velocidades lineales de Lyapunov
	 * dadas las posiciones de origen (actual) y destino (a la que quiero llegar)
	 */
	double velocidadAngularLV(Point3d origen, Point3d destino) {
		double gamma = ParametrosEntorno.GAMMA;
		double beta = ParametrosEntorno.BETA;
		double h = ParametrosEntorno.H;
		
		double theta = getAnguloTheta(origen, destino);
	
		double rotacionActual = origen.z;
		double alpha = getAnguloAlpha(theta, rotacionActual);
	
		double w = gamma * (Math.sin(alpha) + h*theta*Math.sin(alpha)/alpha + beta*alpha);
	
		return w;
	}
	
	/**
	 * Calcula la distancia euclidea entre dos puntos del plano (solamente considera x e y)
	 */
	public double distanciaEuclidea(Point3d origen, Point3d destino) {
		double dist2 = Math.pow(Math.abs(origen.x - destino.x), 2) + Math.pow(Math.abs(origen.y - destino.y), 2);
		
		return Math.sqrt(dist2);
	}
	
	public double getAnguloTheta(Point3d origen, Point3d destino) {
		double catAdyacente = destino.x - origen.x;
		double catOpuesto = destino.y - origen.y;
		
		double theta = 0.0;
		
		if (catAdyacente == 0) {			
			theta = Math.PI/2 * Math.signum(catOpuesto);			
		} else {
			// Calculamos el angulo entre el punto de origen y destino, tomando como referencia
			// la recta que pasa por el punto de partida y tiene la direccion absoluta de angulo 0 del marco de ref
			theta = Math.atan(catOpuesto / catAdyacente);
			
			// Cuando estamos en los cuadrantes 1 y 2 (sin considerar rotacion), el angulo cambia
			if (destino.x < origen.x) {
				// El signo de theta es para saber si tenemos que restar o sumar pi
				double signoTheta = -1.0;
				if (theta != 0.0)
					signoTheta = Math.signum(theta);
				
				// Hay que ir "hacia atras"
				theta = theta - Math.PI*signoTheta;
			}
		}
		
		return theta;		
	}
	
	public double getAnguloAlpha(double theta, double rotacionActual /*, double rotacionDestino*/) {
		double alpha = theta - rotacionActual;
		
		// TODO FALTA TENER EN CUENTA EL ANGULO DE LLEGADA
		
		// Buscamos el menor angulo posible
		if (alpha < -Math.PI)
			alpha = alpha + 2*Math.PI;
		else if (alpha > Math.PI)
			alpha = alpha - 2*Math.PI;
		
		return alpha;
	}

	/** 
	 * Devuelve el cuadrante en el que se encuentra el punto (xOrig, yOrig) con respecto al 
	 * punto (xDest, yDest) si se considera una rotación (rDest) en los ejes 
	 * */	
	public int getCuadrante(Point3d origen, Point3d destino) {
		int cuadrante = 0;
		
		double theta = getAnguloTheta(origen, destino);
		double rDest = destino.z;
		
		
		
		if (theta <= rDest) {
			if (theta + Math.PI/2 <= rDest) {
				cuadrante = 1;
			} else {
				cuadrante = 2;
			}
		} else {
			if (theta - Math.PI/2 >= rDest) {
				cuadrante = 4;
			} else {
				cuadrante = 3;
			}
		}
		
		return cuadrante;
	}
	
	public double getDiferenciaAngulos(double a, double b) {
		double diferencia = 0;
		if (a >= 0 && b >=0 || a <= 0 && b <=0) {
			diferencia = Math.abs((a - b));
		} else if (a >= 0 && b < 0){
			if (a - Math.PI > b)
				diferencia = Math.PI - a + Math.PI + b;
			else
				diferencia = a - b;
		} else {
			if (b - Math.PI > a)
				diferencia = Math.PI - b + Math.PI + a;
			else
				diferencia = b - a;
		}
		
		return diferencia;
	}
	
	
	/**
	 * ATENCION! USAR SOLO EN DetectorAtascamientod
	 * Devuelve la posicion del robot enemigo, girando mi ángulo 45°*
	 */
	public int getCuadranteMuerte(Point3d enemigo, Point3d mi_robot) {
		int cuadrante = 0;
		
		//angulo robot + 45°
		
		double a = mi_robot.z; 
		while(a < 0) {
			a = a + (Math.PI * 2);
		}
		a = a + (Math.PI/4);
		
		//System.out.println("a + 45 = "+ a);
		
		//angulo del oponente con respecto a mi
		double distancia_x;
		if( enemigo.x >= mi_robot.x ) {
			distancia_x = enemigo.x - mi_robot.x;
		} else {
			distancia_x = mi_robot.x - enemigo.x;
		}
		
		double distancia_y;
		if( enemigo.y >= mi_robot.y ) {
			distancia_y = enemigo.y - mi_robot.y;
		} else {
			distancia_y = mi_robot.y - enemigo.y;
		}
		
		double sen_b = sen_b = distancia_y/ distanciaEuclidea(enemigo, mi_robot);;
		double cos_b = cos_b = distancia_x/ distanciaEuclidea(enemigo, mi_robot);
		//System.out.println("sen_b = " + sen_b);
		//System.out.println("cos_b = " + cos_b);
		
		//cos(PI) = -1; sen(PI) = 0
		double cos_PI = -1;
		double sen_PI = 0;
		//recordar que seno(-a) = -sen(a) y cos(a) = cos(-a)
		double cos_menos_b = cos_b; 
		double sen_menos_b = -sen_b;
				
		if( enemigo.x >= mi_robot.x && enemigo.y > mi_robot.y) {
			//B = B
			System.out.println("B = B");
		} else if(enemigo.x < mi_robot.x && enemigo.y >= mi_robot.y) {
			//B = PI - B
			sen_b = cos_menos_b*sen_PI + sen_menos_b*cos_PI;
			cos_b = cos_menos_b*cos_PI - sen_menos_b*sen_PI;
			//System.out.println("B = PI - B");
		} else if(enemigo.x <= mi_robot.x && enemigo.y < mi_robot.y) {
			//B = PI + B
			sen_b = cos_b*sen_PI + sen_b*cos_PI;
			cos_b = cos_b*cos_PI - sen_b*sen_PI;
			//System.out.println("B = PI + B");
		} else {
			//B = -B
			sen_b = sen_menos_b;
			cos_b = cos_menos_b;
			//System.out.println("B = -B");
		}
		
		double sen_a = Math.sin(-a);
		double cos_a = Math.cos(-a);
		
		//seno de b - (a + 45)
		double sen_res = cos_b*sen_a + sen_b*cos_a; 
		double cos_res = cos_b*cos_a - sen_b*sen_a;
		//System.out.println("sen_a = " + sen_a + " cos_a = " + cos_a + " sen_b = " + sen_b + " cos_b = " + cos_b);
		//System.out.println("sen_res = " + sen_res + " cos_res = " + cos_res);
		if(cos_res > 0 && sen_res >= 0) {
			cuadrante = 1;
		} else if(cos_res <= 0 && sen_res > 0){
			cuadrante = 2;
		} else if(cos_res < 0 && sen_res <= 0){
			cuadrante = 3;
		} else {
			cuadrante = 4;
		}		
				
		return cuadrante;
	}
	
	public String formatearPunto(Point3d p) {
		return ("(" + p.x + ", " + p.y + ", " + p.z + ")");
	}
}